#include "TmQPlateCalculator.h"
#include "SGFem/Integration/IntegrationFormula/Gauss2D2X2P.h"

using SG::DataStructure::Common::Real;
using SG::Algebra::Matrix;
using SG::DataStructure::FEM::ShellSecMaterialConstant;

void TmQPlateCalculator::Initialize ()
{
    SG::FEMSolver::Element::Gauss2D2X2P integForm;
    auto integ = integForm.GetPoints();
    m_shpFunc.Compute (integ);
    m_shpFunc8.Compute (integ);
}

void TmQPlateCalculator::SetCoordinate (const Matrix& localCoord,const ShellSecMaterialConstant& SecConstant)
{
    Matrix Cb, Cs;    
    auto tp = SecConstant.m_t;
    SecConstant.GetCs(tp, Cs);
    SecConstant.GetCb(tp, Cb);


    auto b1 = localCoord (1, 1) - localCoord (2, 1);
    auto b2 = localCoord (2, 1) - localCoord (3, 1);
    auto b3 = localCoord (3, 1) - localCoord (0, 1);
    auto b4 = localCoord (0, 1) - localCoord (1, 1);

    auto c1 = localCoord (2, 0) - localCoord (1, 0);
    auto c2 = localCoord (3, 0) - localCoord (2, 0);
    auto c3 = localCoord (0, 0) - localCoord (3, 0);
    auto c4 = localCoord (1, 0) - localCoord (0, 0);

    // squre of the length of element side
    std::vector<Real> squreL{ b1 * b1 + c1 * c1, b2 * b2 + c2 * c2, b3 * b3 + c3 * c3, b4 * b4 + c4 * c4};

    std::vector<Real> delta (4);
    Matrix d(1,2),T1(3,3),T2(2,2);
    for (int iSide = 0; iSide < 4; ++iSide)
    {
        Real lambda;
        if (SecConstant.m_isComposite)
        {
            Real beta;
            if (0 == iSide)
            {      
                //d = [x3-x2, y3-y2]; d = d/norm(d);  beta(1) = acos(d*[1;0]);
                d(0,0)=localCoord(2,0)-localCoord(1,0);
                d(0,1)=localCoord(2,1)-localCoord(1,1);
            }
            else if (1 == iSide)
            {
                // d = [x4-x3, y4-y3]; d = d/norm(d);  beta(2) = acos(d*[1;0]);
                d(0,0) =localCoord(3,0)-localCoord(2,0);
                d(0,1)= localCoord(3,1)-localCoord(2,1);
            }
            else if (2 == iSide)
            {
                //d = [x1-x4, y1-y4]; d = d/norm(d);  beta(3) = acos(d*[1;0]);
                d(0,0) =localCoord(0,0)-localCoord(3,0);
                d(0,1)= localCoord(0,1)-localCoord(3,1);
            }
            else
            {
                //d = [x2-x1, y2-y1]; d = d/norm(d);  beta(4) = acos(d*[1;0]);
                d(0,0) =localCoord(1,0)-localCoord(0,0);
                d(0,1)= localCoord(1,1)-localCoord(0,1);
            }
            // 方向单位化
            Real dmod=sqrt(d(0,0)*d(0,0)+d(0,1)*d(0,1));
            d(0,0)=d(0,0)/dmod;
            d(0,1)=d(0,1)/dmod;

            beta = acos(d(0,0));

            Real c = cos(beta); 
            Real s = sin(beta);
            SG::DataStructure::FEM::calcMatCoordTransMatrix(c, s, T1, T2);

            Matrix D_i = T1*Cb*T1.Transpose();
            
            Matrix Cs_i = T2*Cs*T2.Transpose();

            lambda = D_i(1,1)/(Cs_i(1,1)*squreL[iSide]);
        }
        else
            lambda = Cb (0, 0) / Cs (0, 0) / squreL[iSide];

        delta[iSide] = 6.0 / (1.0 / lambda + 12.0);
    }

    // 取倒数
    squreL[0] = 1.0/squreL[0];
    squreL[1] = 1.0/squreL[1];
    squreL[2] = 1.0/squreL[2];
    squreL[3] = 1.0/squreL[3];

    // m_alpha = [alpha1 alpha2 alpha3 alpha4];      
    m_alpha = Matrix (4, 12);
    // alpha1
    m_alpha(2, 0) = 1.5*c3*(1-2*delta[2])*squreL[2]; m_alpha(2,  1) = 0.5*(b3*b3-0.5*c3*c3*(1-6*delta[2]))*squreL[2]; m_alpha(2,  2) = 0.75*c3*b3*(1-2*delta[2])*squreL[2];
    m_alpha(3, 0) =-1.5*c4*(1-2*delta[3])*squreL[3]; m_alpha(3,  1) = 0.5*(b4*b4-0.5*c4*c4*(1-6*delta[3]))*squreL[3]; m_alpha(3,  2) = 0.75*c4*b4*(1-2*delta[3])*squreL[3];
    // alpha2
    m_alpha(0, 3) =-1.5*c1*(1-2*delta[0])*squreL[0]; m_alpha(0,  4) = 0.5*(b1*b1-0.5*c1*c1*(1-6*delta[0]))*squreL[0]; m_alpha(0,  5) = 0.75*c1*b1*(1-2*delta[0])*squreL[0];
    m_alpha(3, 3) = 1.5*c4*(1-2*delta[3])*squreL[3]; m_alpha(3,  4) = 0.5*(b4*b4-0.5*c4*c4*(1-6*delta[3]))*squreL[3]; m_alpha(3,  5) = 0.75*c4*b4*(1-2*delta[3])*squreL[3]; 
    // alpha3
    m_alpha(0, 6) = 1.5*c1*(1-2*delta[0])*squreL[0]; m_alpha(0,  7) = 0.5*(b1*b1-0.5*c1*c1*(1-6*delta[0]))*squreL[0]; m_alpha(0,  8) = 0.75*c1*b1*(1-2*delta[0])*squreL[0];
    m_alpha(1, 6) =-1.5*c2*(1-2*delta[1])*squreL[1]; m_alpha(1,  7) = 0.5*(b2*b2-0.5*c2*c2*(1-6*delta[1]))*squreL[1]; m_alpha(1,  8) = 0.75*c2*b2*(1-2*delta[1])*squreL[1];
    // alpha4
    m_alpha(1, 9) = 1.5*c2*(1-2*delta[1])*squreL[1]; m_alpha(1, 10) = 0.5*(b2*b2-0.5*c2*c2*(1-6*delta[1]))*squreL[1]; m_alpha(1, 11) = 0.75*c2*b2*(1-2*delta[1])*squreL[1];
    m_alpha(2, 9) =-1.5*c3*(1-2*delta[2])*squreL[2]; m_alpha(2, 10) = 0.5*(b3*b3-0.5*c3*c3*(1-6*delta[2]))*squreL[2]; m_alpha(2, 11) = 0.75*c3*b3*(1-2*delta[2])*squreL[2];


    //  m_beta = [beta1 beta2 beta3 beta4];
    m_beta = Matrix (4, 12);
    // beta1
    m_beta(2, 0) =-1.5*b3*(1-2*delta[2])*squreL[2]; m_beta(2, 1) = 0.75*c3*b3*(1-2*delta[2])*squreL[2]; m_beta(2,  2) = 0.5*(c3*c3-0.5*b3*b3*(1-6*delta[2]))*squreL[2]; 
    m_beta(3, 0) = 1.5*b4*(1-2*delta[3])*squreL[3]; m_beta(3, 1) = 0.75*c4*b4*(1-2*delta[3])*squreL[3]; m_beta(3,  2) = 0.5*(c4*c4-0.5*b4*b4*(1-6*delta[3]))*squreL[3];
    // beta2
    m_beta(0, 3) = 1.5*b1*(1-2*delta[0])*squreL[0]; m_beta(0, 4) = 0.75*c1*b1*(1-2*delta[0])*squreL[0]; m_beta(0,  5) = 0.5*(c1*c1-0.5*b1*b1*(1-6*delta[0]))*squreL[0];
    m_beta(3, 3) =-1.5*b4*(1-2*delta[3])*squreL[3]; m_beta(3, 4) = 0.75*c4*b4*(1-2*delta[3])*squreL[3]; m_beta(3,  5) = 0.5*(c4*c4-0.5*b4*b4*(1-6*delta[3]))*squreL[3];
    // beta3
    m_beta(0, 6) =-1.5*b1*(1-2*delta[0])*squreL[0]; m_beta(0, 7) = 0.75*c1*b1*(1-2*delta[0])*squreL[0]; m_beta(0,  8) = 0.5*(c1*c1-0.5*b1*b1*(1-6*delta[0]))*squreL[0];
    m_beta(1, 6) = 1.5*b2*(1-2*delta[1])*squreL[1]; m_beta(1, 7) = 0.75*c2*b2*(1-2*delta[1])*squreL[1]; m_beta(1,  8) = 0.5*(c2*c2-0.5*b2*b2*(1-6*delta[1]))*squreL[1]; 
    // beta4
    m_beta(1, 9) =-1.5*b2*(1-2*delta[1])*squreL[1]; m_beta(1, 10) =0.75*c2*b2*(1-2*delta[1])*squreL[1]; m_beta(1, 11) = 0.5*(c2*c2-0.5*b2*b2*(1-6*delta[1]))*squreL[1];
    m_beta(2, 9) = 1.5*b3*(1-2*delta[2])*squreL[2]; m_beta(2, 10) =0.75*c3*b3*(1-2*delta[2])*squreL[2]; m_beta(2, 11) = 0.5*(c3*c3-0.5*b3*b3*(1-6*delta[2]))*squreL[2];
 
    // Gamma = diag(delta) *G;
    Matrix Gamma(4, 12, {
                    0,            0,            0,   -2*delta[0], -b1*delta[0], -c1*delta[0],    2*delta[0], -b1*delta[0], -c1*delta[0],              0,            0,   0,
                    0,            0,            0,             0,            0,            0,   -2*delta[1], -b2*delta[1], -c2*delta[1],     2*delta[1], -b2*delta[1], -c2*delta[1],
           2*delta[2], -b3*delta[2], -c3*delta[2],             0,            0,            0,             0,            0,            0,    -2*delta[2], -b3*delta[2], -c3*delta[2],
          -2*delta[3], -b4*delta[3], -c4*delta[3],    2*delta[3], -b4*delta[3], -c4*delta[3],             0,            0,            0,              0,            0,   0});


    m_Xs = Matrix(4, 4, { 
                   0,                 0, -b4/(b3*c4-b4*c3),  b3/(b3*c4-b4*c3),
    b4/(b4*c1-b1*c4),                 0,                 0, -b1/(b4*c1-b1*c4),
   -b2/(b1*c2-b2*c1),  b1/(b1*c2-b2*c1),                 0,                 0,
                  0 , -b3/(b2*c3-b3*c2),  b2/(b2*c3-b3*c2),                 0});

    m_Xs = m_Xs * Gamma;

    m_Ys = Matrix(4, 4, {          
                    0,                 0, -c4/(b3*c4-b4*c3),  c3/(b3*c4-b4*c3),
     c4/(b4*c1-b1*c4),                 0,                 0, -c1/(b4*c1-b1*c4),
    -c2/(b1*c2-b2*c1),  c1/(b1*c2-b2*c1),                 0,                 0,
                    0, -c3/(b2*c3-b3*c2),  c2/(b2*c3-b3*c2),                 0});
                    
    m_Ys = m_Ys * Gamma;

}

void TmQPlateCalculator::ComputeB (int iGauss, const SG::Algebra::Matrix& localCoord, Matrix& Bs, Matrix& Bb, Real& detj) const
{
    // shear
    auto NT = m_shpFunc.GetShapeFunction (iGauss).Transpose(); 
    Bs = Matrix(2, 12);
    Bs.BlockFill (0, 0, NT*m_Xs);
    Bs.BlockFill (1, 0, NT*m_Ys);

    
    // % bending strain matrix
    const auto& DNDxi = m_shpFunc.GetShapeFunctionDerivative(iGauss);
    auto JT = DNDxi * localCoord;
    detj = JT(0, 0) * JT(1, 1) - JT(0, 1) * JT(1, 0);

    auto JTinv = JT.Inverse();
    // J = obj.shpFunc(iGauss).DNDxi*obj.LocCoord(:,1:2);
    // detj = det(J);
    auto DN8Dx = JTinv * m_shpFunc8.GetShapeFunctionDerivative(iGauss);
    auto H0 = Matrix(3,12);
    auto H1 = Matrix(3,4);
    auto H2 = Matrix(3,4);
    // DN8Dx_n = DN8Dx(:,[6,7,8,5]);
    std::vector <int> DN8Dx_n {5,6,7,4};
    for (std::size_t iNode = 0; iNode < 4; ++iNode)
    {
         H0(0, iNode*3+1) = DN8Dx(0, iNode);
         H0(1, iNode*3+2) = DN8Dx(1, iNode);
         H0(2, iNode*3+1) = DN8Dx(1, iNode);
         H0(2, iNode*3+2) = DN8Dx(0, iNode);
         
         H1(0, iNode) = DN8Dx (0, DN8Dx_n[iNode]);
         H1(2, iNode) = DN8Dx (1, DN8Dx_n[iNode]);         
         H2(1, iNode) = DN8Dx (1, DN8Dx_n[iNode]);
         H2(2, iNode) = DN8Dx (0, DN8Dx_n[iNode]);
    }

    
    Matrix I0 (3, 3, {1, 0, 0,  0, 0, -1,  0, 1, 0});
    Matrix L0 (12, 12);
    L0.BlockFill (0, 0, I0);
    L0.BlockFill (3, 3, I0);
    L0.BlockFill (6, 6, I0);
    L0.BlockFill (9, 9, I0);

    Bb = (H0 + H1*m_alpha + H2*m_beta)* L0 *-1.0;
}